#include "../Device/cmd.h"

#include <stdio.h>
#include <string.h>

/* 命令格式 字符+RM */
/* 如 "sRM" 表示停止命令，电机停止输出*/

char tmp[20];

void CMD_Init() { BSP_Uart_Init(); }

CMD_t CMD_PraseData() {
  if (!BSP_Uart_CheckFlag()) return CMD_NULL;
  uint8_t* tmp = BSP_Uart_GetDataAddr();
  if (strcmp("RM", (const char*)(tmp + 1))) {
    memset(tmp, 0, BUFFER_SIZE);
    return CMD_NULL;
  }
  switch (tmp[0]) {
    case 's':
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_STOP;
    case 'r':
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_RESET;
    case 'f':
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_TEST_LED;
    case 't':
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_EASY;
    case 'e':
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_NORMAL;
    case 'a':
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_ANTI;
    default:
      memset(tmp, 0, BUFFER_SIZE);
      return CMD_NULL;
  }
}

void CMD_Report(float setpoint, float fb) {
  sprintf(tmp, "%.2f,%.2f\r\n", setpoint, fb);
  BSP_Uart_StartSend((uint8_t*)tmp, strlen(tmp));
}
